/********************************************
* Copyrigt (C) rumi
* Module: mgr_trace.cpp
*********************************************/
#include "mgr_trace.h"
#include <QThread>

Trace_RecvData_Typedef RecDataFromRos;
Trace_SendData_Typedef SendDataToRos;
Trace_SendFlag_Typedef SendFlag;

void receiveDataFromShm() {
    // 打开接收共享内存
    int shm_recv_fd = shm_open(SHM_NAME_RECV, O_RDWR, 0666);
    if (shm_recv_fd == -1) {
        perror("Error opening SHM_NAME_RECV");
        return;
    }

    SharedMemory_Recv_TypeDef *sharedMemoryRecv = (SharedMemory_Recv_TypeDef *)mmap(nullptr, sizeof(SharedMemory_Recv_TypeDef), PROT_READ | PROT_WRITE, MAP_SHARED, shm_recv_fd, 0);
    if (sharedMemoryRecv == MAP_FAILED) {
        perror("Error mapping SHM_NAME_RECV");
        close(shm_recv_fd);
        return;
    }
    std::cout << "Shared memory address in app2: " << sharedMemoryRecv << std::endl;
    //std::cout << "Shared memory data: " << sharedMemoryRecv->Data << std::endl;
    while (true) {
        int semValue;
        sem_getvalue(&sharedMemoryRecv->Sem_Notice, &semValue);
        //std::cout << "Semaphore value before wait: " << semValue << std::endl;

        sem_wait(&sharedMemoryRecv->Sem_Notice);

        if (sharedMemoryRecv->DataState == DataState_Working)
        {
            // auto now = std::chrono::system_clock::now();
            // std::time_t now_time = std::chrono::system_clock::to_time_t(now);
            // auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
            // std::ostringstream timestampStream;
            // timestampStream << std::put_time(std::localtime(&now_time), "%Y-%m-%d %H:%M:%S") << "." << std::setfill('0') << std::setw(3) << milliseconds.count();
            // std::string timestamp = timestampStream.str();
            // std::string receivedData = "[" + timestamp + "] " +
            //                    std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.SoftWareVer)) + "." +
            //                    std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.SoftWareBuildVer)) + " " +
            //                    std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.HardWareVer)) + " " +
            //                    std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.BootLoaderVer));

            // std::cout << "Received: "<< receivedData << std::endl;
            switch(sharedMemoryRecv->DataType)
            {
                case E_CMD_READVER:  // Ver
                {
                    RecDataFromRos.Ver = sharedMemoryRecv->Data.Ver;
                    break;
                }

                case E_CMD_HEARTBEAT:  // heartbeat
                {
                    break;
                }
                case E_CMD_PID_LEFT:
                {
                    RecDataFromRos.Pid_Left = sharedMemoryRecv->Data.Pid_Left;
                    break;
                }
                case E_CMD_PID_RIGHT:
                {
                    RecDataFromRos.Pid_Right = sharedMemoryRecv->Data.Pid_Right;
                    break;
                }
                case E_CMD_PID_STAND:
                {
                    RecDataFromRos.Pid_Stand = sharedMemoryRecv->Data.Pid_Stand;
                    break;
                }
                case E_CMD_PID_STER:
                {
                    RecDataFromRos.Pid_Ster = sharedMemoryRecv->Data.Pid_Ster;
                    break;
                }
                default:
                {
                    break;
                }
            }
            sharedMemoryRecv->DataState = DataState_Idle;  
        }
        //std::this_thread::sleep_for(std::chrono::milliseconds(10));
        QThread::msleep(5);
    }

    munmap(sharedMemoryRecv, sizeof(SharedMemory_Recv_TypeDef));
    close(shm_recv_fd);
}

void sendDataToShm() {
    int shm_send_fd = shm_open(SHM_NAME_SEND, O_RDWR, 0666);
    if (shm_send_fd == -1) {
        perror("Error opening SHM_NAME_SEND");
        std::cerr << "Error: Unable to open shared memory for sending." << std::endl;
        return;
    }

    SharedMemory_Send_TypeDef *sharedMemorySend = (SharedMemory_Send_TypeDef *)mmap(nullptr, sizeof(SharedMemory_Send_TypeDef), PROT_READ | PROT_WRITE, MAP_SHARED, shm_send_fd, 0);
    if (sharedMemorySend == MAP_FAILED) {
        std::cerr << "Error: Unable to map shared memory for sending." << std::endl;
        close(shm_send_fd);
        return;
    }
    sharedMemorySend->DataState = DataState_Idle;
    //int counter = 0;
    while (true) {
        if(sharedMemorySend->DataState == DataState_Idle) 
        {
            if(SendDataToRos.Ctrl.ReadVer == 1)
            {
                SendDataToRos.Ctrl.ReadVer = 0;
                sharedMemorySend->DataType = E_CMD_READVER;
                sharedMemorySend->Data.Ctrl.ReadVer = 1;

                sharedMemorySend->DataState = DataState_Working;
                sem_post(&sharedMemorySend->Sem_Notice);
            }
        }
        QThread::msleep(1);
    }

    munmap(sharedMemorySend, sizeof(SharedMemory_Send_TypeDef));
    close(shm_send_fd);
}

/********************************************
 * End of file
*********************************************/
